#include "graph.h"
#include <iostream>

using namespace std;

Graph::Graph(){}
Graph::~Graph(){}

vector<Point*> Graph::getPoints(){
	return this->points;
}

Point* Graph::createPoint(Point* p){
	this->points.push_back(p);
	return p;
}

Point* Graph::createPoint(int x, int y){
	Point* p = new Point(x, y);
        return this->createPoint(p);
}

void Graph::removePoint(Point* p){
	vector<Link*>::iterator currentL = this->links.begin();
	vector<Point*>::iterator currentP = this->points.begin();
	while( currentL != this->links.end() ){
		this->removeLink(*currentL);
		currentL++;
	}
	while( currentP != this->points.end() ){
		if((*currentP) == p){
			this->points.erase(currentP);
			currentP = this->points.end();
		}
		currentP++;
	}
	delete p;
}

Link* Graph::createLink(Point* p1, Point* p2){
	Link* tmp = NULL;
	if((*p1) != (*p2)){
                this->links.push_back(new Link(p1,p2));
	}
	return tmp;
}
void Graph::removeLink(Link* l){
	vector<Link*>::iterator current = this->links.begin();
	while( current != this->links.end() ){
		if( (*current) == l ){
			this->links.erase( current );
			current = this->links.end();
		}
		if(current != this->links.end()){
			current++;
		}
	}
	delete l;
}

vector<Link*> Graph::getLinks(Point* search){
	vector<Link*> vect;
	vector<Link*>::iterator current = this->links.begin();
	while( current != this->links.end() ){
		if(search == NULL || (*(*current)->getP1()) == *search || (*(*current)->getP2()) == *search ){
			vect.push_back((*current));
		}
		current++;
	}
	return vect;
}

ostream & operator<<( ostream &out, Graph gr){
	vector<Link*> links = gr.getLinks();
	vector<Link*>::iterator current = links.begin();
	while(current != links.end()){
                out << (**current) << endl;
		current++;
	}
	return out;
}

vector<Link*> Graph::astar(Point* start, Point* goal){
	vector<Link*> open_list;
	Link* min = NULL;
	Point* c_pt = start;
	vector<Link*> links = this->getLinks(start);
	vector<Link*>::iterator current = links.begin();	
	min = links[0];
	
	// On calcule donc la distance entre le point étudié et le dernier point qu'on a jugé comme bon. 
	// La somme de ces deux distances nous donne la qualité du noeud étudié.
	// Plus un noeud à une qualité faible, meilleur il est.
	while(*c_pt != *goal){
		cout << endl << endl << endl << endl;
		
		while(current != links.end()){
/*			cout << "======================================================" << endl;		
			cout << "Current" << *(*current) << endl; 
			cout << Link::getWeight(*current) << " " << Link::getWeight(getOutPoint(*current, c_pt), goal) << endl;
			cout << "min" << *min << endl;			
			cout << Link::getWeight(min) << " " << Link::getWeight(getOutPoint(min, c_pt), goal) << endl;
			cout << "======================================================" << endl;		
*/			
			if(Link::getWeight(*current) + Link::getWeight(getOutPoint(*current, c_pt), goal) < 
			Link::getWeight(min) + Link::getWeight(getOutPoint(min, c_pt), goal)){
				min = *current;
                        }
			current++;					
		}
		//cout << *min << endl;
		cout << "======================================================" << endl;		
                open_list.push_back(min);
		c_pt = getOutPoint(min, c_pt);
		cout << *c_pt << endl;
		links = this->getLinks(c_pt);		
		current = links.begin();
		while(current != links.end()){
			cout << *(*current) << endl;
			current++;
		}
		
		current = links.begin();
		min = links[0];
	}
	return open_list;
}

Point* Graph::getOutPoint(Link* link, Point* current){
	return (*current == *(link->getP1())) ? link->getP2() : link->getP1() ;
}
